/*
 * pid.c
 *
 *  Created on: 2025年6月8日
 *      Author: jwhww
 */
#include "AllHeadFile.h"
//==================================================定义 pid目标结构体===============================================
//创建pid目标，我帮你创好了
//PID_MOTOR Struct_Pid_GM6020[8] = {0};
// PID_MOTOR Struct_Pid_M3508[8] = {0};
// PID_MOTOR Struct_Pid_M2006[8] = {0};
// MIT控制
// pidTypedef Struct_Pid_GM6020_MIT_TORQUE;    pidTypedef Struct_Pid_GM6020_MIT_POSITION;    pidTypedef Struct_Pid_GM6020_MIT_SPEED;    //力矩、位置、速度
//==================================================定义 pid目标结构体===============================================
//专属于pid的钳位函数
static pidtype clip_pidtype(const pidtype x, const pidtype low, const pidtype up) {
    return x > up ? up : x < low ? low : x;
}

//------------------------------------------------------------------------------------------------------
// 函数简介     pid单结构体初始化
// 使用示例     pid_sample_init(&Struct_Pid_GM6020_Yaw, 0.0001f, 0, 0, Incremental, 3000, 16384);
// 返回参数     void
// 备注信息     内部调用
//------------------------------------------------------------------------------------------------------
static void pid_sample_init(pidTypedef *struct_pid, const pidtype Kp, const pidtype Ki, const pidtype Kd, const pidtype mode, const pidtype Ki_out_limit, const pidtype out_limit) {
    //初始化赋值，目标结构体请到pid.c前几行寻找
    //电机平均速度
    struct_pid->Kp = Kp;   //必须强制转换一次,因为如果传入的是int,会无法转为pidtype从而赋值失败
    struct_pid->Ki = Ki;
    struct_pid->Kd = Kd;
    struct_pid->integral_limit = (pidtype)Ki_out_limit;    //积分限幅
    struct_pid->error_integral = 0;
    struct_pid->error_last = 0;
    struct_pid->error_last_last = 0;
    struct_pid->output_accumulation = 0;
    struct_pid->out_limit = out_limit;  //绝对值
    struct_pid->mode = mode;      //增量式
    struct_pid->target_last = 0;    //上一次的目标值(前馈用)
}

//------------------------------------------------------------------------------------------------------
// 函数简介     pid创建实例
// 使用示例     pidTypedef* pid = pid_sample_create(0.0001f, 0, 0, Incremental, 3000, 16384);
// 返回参数     pidTypedef*
// 备注信息     内部调用(开发中，切勿使用），无法创建全局结构体
//------------------------------------------------------------------------------------------------------
static pidTypedef* pid_sample_create(const pidtype Kp, const pidtype Ki, const pidtype Kd, const pidmode mode, const pidtype Ki_out_limit, const pidtype out_limit) {
    pidTypedef* pid = (pidTypedef*)malloc(sizeof(pidTypedef));  //stack里够放的
    if (pid == NULL) {
        return NULL;
    }
    pid_sample_init(pid, Kp, Ki, Kd, mode, Ki_out_limit, out_limit);
    return pid;
}

//------------------------------------------------------------------------------------------------------
// 函数简介     pid初始化
// 使用示例     pid_init();
// 返回参数     void
// 备注信息       ----------关于PID输出限幅请看下面----------
// 备注信息       电压给定值范围：-25000~0~25000，对应±24V
// 备注信息       电流给定值范围：-16384~0~16384, 对应最大转矩电流范围-3A~0~3A。注意！连续负载最大1.62A（±8852），对应最大连续扭矩1.2N·m，不然会烧
// 备注信息       堵转安全电流0.9A（±4915），检测到堵转之后请手动调整电流到0.9A（±4915）以下
// 备注信息       关于“堵转”和“带载”的定义，请结合需求自己确定
// 备注信息       ----------关于PID输出限幅请看上面----------
//------------------------------------------------------------------------------------------------------
void pid_init(void){
    //MIT,POSITION和SPEED都只使用KP
    pid_sample_init(&Motor_Info_GM6020[0].Struct_Pid.MIT.POSITION, 5, 0, 0, Positional, 400, 10000);
    pid_sample_init(&Motor_Info_GM6020[0].Struct_Pid.MIT.SPEED, 40, 0, 0, Positional, 800, 10000);
    // pid_sample_init(&Motor_Info_GM6020[1].Struct_Pid.MIT.POSITION, 4, 0, 0, Positional, 800, 10000);
    // pid_sample_init(&Motor_Info_GM6020[1].Struct_Pid.MIT.SPEED, 50, 0, 0, Positional, 800, 10000);
    // pid_sample_init(&Motor_Info_GM6020[2].Struct_Pid.MIT.POSITION, 4, 0, 0, Positional, 800, 10000);
    // pid_sample_init(&Motor_Info_GM6020[2].Struct_Pid.MIT.SPEED, 50, 0, 0, Positional, 800, 10000);
    // pid_sample_init(&Motor_Info_GM6020[3].Struct_Pid.MIT.POSITION, 4, 0, 0, Positional, 800, 10000);
    // pid_sample_init(&Motor_Info_GM6020[3].Struct_Pid.MIT.SPEED, 50, 0, 0, Positional, 800, 10000);

//------------------------------------------------------------------GM6020----------------------------------------------------------------------
    //一般参数（可选，很通用，可用作串并结合时给单独并环用）
    pid_sample_init(&Motor_Info_GM6020[0].Struct_Pid.NORMAL, 0.5f, 0.05f, 0, Positional, 800, 10000);
    pid_sample_init(&Motor_Info_GM6020[1].Struct_Pid.NORMAL, 0.9f, 0, 0, Positional, 400, 500);
    pid_sample_init(&Motor_Info_GM6020[2].Struct_Pid.NORMAL, 1, 0, 5, Positional, 400, 14000);
    pid_sample_init(&Motor_Info_GM6020[3].Struct_Pid.NORMAL, 0.9f, 0, 0, Positional, 400, 500);
    pid_sample_init(&Motor_Info_GM6020[4].Struct_Pid.NORMAL, 0, 0, 0, Positional, 800, 10000);
    pid_sample_init(&Motor_Info_GM6020[5].Struct_Pid.NORMAL, 0, 0, 0, Positional, 800, 10000);
    pid_sample_init(&Motor_Info_GM6020[6].Struct_Pid.NORMAL, 0, 0, 0, Positional, 800, 10000);
    pid_sample_init(&Motor_Info_GM6020[7].Struct_Pid.NORMAL, 0, 0, 0, Positional, 800, 10000);
    //单独特定参数（串级或单级都可用，好区分好辨认）
    pid_sample_init(&Motor_Info_GM6020[0].Struct_Pid.POSITION, 0.9f, 0, 2.9f, Positional, 500, 320);  //kp=0.05 实际上导致速度环最大输入只有33（遥控器输入最大660）
    pid_sample_init(&Motor_Info_GM6020[0].Struct_Pid.SPEED, 250, 0.0f, 1.f, Positional, 9000, 16000);   //out_limit实际上在限制力矩输出
    pid_sample_init(&Motor_Info_GM6020[0].Struct_Pid.TORQUE, 0.5f, 0.1f, 0, Incremental, 500, 10000);

    pid_sample_init(&Motor_Info_GM6020[1].Struct_Pid.POSITION, 0.4f, 0.f, 0.08f, Positional, 400, 320);    ////kp=0.08 实际上导致速度环最大输入只有 52.8（遥控器输入最大660）
    pid_sample_init(&Motor_Info_GM6020[1].Struct_Pid.SPEED, 190.0f, 10.0f, 0.f, Positional, 10000, 16000);
    pid_sample_init(&Motor_Info_GM6020[1].Struct_Pid.TORQUE, 0.7f, 0.1f, 0.f, Incremental, 500, 10000);

    pid_sample_init(&Motor_Info_GM6020[2].Struct_Pid.POSITION, 0.1f, 0.f, 5.f, Positional, 400, 320);
    pid_sample_init(&Motor_Info_GM6020[2].Struct_Pid.SPEED, 15.0f, 3.0f, 0.f, Incremental, 400, 10000);
    pid_sample_init(&Motor_Info_GM6020[2].Struct_Pid.TORQUE, 0.7f, 0.1f, 0.f, Incremental, 500, 10000);

    pid_sample_init(&Motor_Info_GM6020[3].Struct_Pid.POSITION, 0, 0, 0, Positional, 800, 10000);
    pid_sample_init(&Motor_Info_GM6020[3].Struct_Pid.SPEED, 0, 0, 0, Positional, 800, 10000);
    pid_sample_init(&Motor_Info_GM6020[3].Struct_Pid.TORQUE, 0.7f, 0.1f, 0, Incremental, 500, 10000);

    pid_sample_init(&Motor_Info_GM6020[4].Struct_Pid.POSITION, 0, 0, 0, Positional, 800, 10000);
    pid_sample_init(&Motor_Info_GM6020[4].Struct_Pid.SPEED, 0, 0, 0, Positional, 800, 10000);
    pid_sample_init(&Motor_Info_GM6020[4].Struct_Pid.TORQUE, 0.7f, 0.1f, 0, Incremental, 500, 10000);

    pid_sample_init(&Motor_Info_GM6020[5].Struct_Pid.POSITION, 0, 0, 0, Positional, 800, 10000);
    pid_sample_init(&Motor_Info_GM6020[5].Struct_Pid.SPEED, 0, 0, 0, Positional, 800, 10000);
    pid_sample_init(&Motor_Info_GM6020[5].Struct_Pid.TORQUE, 0.7f, 0.1f, 0, Incremental, 500, 10000);

    pid_sample_init(&Motor_Info_GM6020[6].Struct_Pid.POSITION, 0, 0, 0, Positional, 800, 10000);
    pid_sample_init(&Motor_Info_GM6020[6].Struct_Pid.SPEED, 0, 0, 0, Positional, 800, 10000);
    pid_sample_init(&Motor_Info_GM6020[6].Struct_Pid.TORQUE, 0.7f, 0.1f, 0, Incremental, 500, 10000);

    pid_sample_init(&Motor_Info_GM6020[7].Struct_Pid.POSITION, 0, 0, 0, Positional, 800, 10000);
    pid_sample_init(&Motor_Info_GM6020[7].Struct_Pid.SPEED, 0, 0, 0, Positional, 800, 10000);
    pid_sample_init(&Motor_Info_GM6020[7].Struct_Pid.TORQUE, 0.7f, 0.1f, 0, Incremental, 500, 10000);

    //------------------------------------------------------------------M3508----------------------------------------------------------------------
    pid_sample_init(&Motor_Info_M3508[0].Struct_Pid.NORMAL, 0.5f, 0, 0, Incremental, 800, 10000);
    pid_sample_init(&Motor_Info_M3508[0].Struct_Pid.POSITION, 10, 0, 0, Positional, 800, 10000);
    pid_sample_init(&Motor_Info_M3508[0].Struct_Pid.SPEED, 10.f, 0.5f, 0.f, Incremental, 800, 10000);
    pid_sample_init(&Motor_Info_M3508[0].Struct_Pid.TORQUE, 0.5f, 0.01f, 0.1f, Incremental, 500, 10000);

}

//------------------------------------------------------------------------------------------------------
// 函数简介     前馈计算（target-target_last）
// 参数介绍     Struct_Pid      pid目标结构体地址
// 参数介绍     target          目标值
// 参数介绍     target_last     上一次的目标值
// 使用示例     pid_frontFeed(&Struct_Pid_GM6020_1, 10, target);
// 返回参数     output                          位置式pid计算结果
// 备注信息     实验性
//------------------------------------------------------------------------------------------------------
pidtype pid_frontFeed(pidTypedef* Struct_Pid, pidtype K, pidtype target) {
    const pidtype output = K * (target - Struct_Pid->target_last);
    Struct_Pid->target_last = target;
    return output;
}

//------------------------------------------------------------------------------------------------------
// 函数简介     pid计算并输出（target-current）
// 参数介绍     Struct_Pid      pid目标结构体地址
// 参数介绍     target          目标值
// 参数介绍     current         当前值
// 参数介绍     out_limit       pid输出限幅（上下都限）
// 使用示例     Pid_Calculate_Tar_Cur(&Struct_Pid_Motor_Ave, target_speed, (left_wheel_speed_n_s + right_wheel_speed_n_s)/2, 2000);
// 返回参数     output                          位置式pid计算结果
// 返回参数     hpid ->output_accumulation      增量式pid计算结果
// 备注信息     数据类型由pid.h中的pidtype定义；注意每次使用前观察积分限幅有关的代码
//------------------------------------------------------------------------------------------------------
pidtype pid_calculate_tar_cur(pidTypedef* Struct_Pid, pidtype target, pidtype current)   //pidtype在文件头定义，可自定义，需要啥就用啥，默认为float
{
    pidtype error = target - current;
    //位置式
    if (Struct_Pid->mode == Positional)
    {
        if (Struct_Pid -> Ki != 0) {
            Struct_Pid ->error_integral += error ;
        }
        //离散积分累加项
        else {
            Struct_Pid ->error_integral = 0;    //积分项为0时，积分项输出为0
        }

        pidtype output = Struct_Pid -> Kp * error
                       + clip_pidtype(Struct_Pid -> Ki * Struct_Pid -> error_integral, -Struct_Pid -> integral_limit, Struct_Pid -> integral_limit) //积分限幅(非归一化）：注意积分限幅需要使用原始数据，需要和output同单位
                       + Struct_Pid -> Kd * (error - Struct_Pid ->error_last);
        Struct_Pid->error_last = error;                                                                                           //离散微分需要使用的后向差分项
        //PID输出限幅
        if(output > Struct_Pid->out_limit) output = Struct_Pid->out_limit;
        if(output < -Struct_Pid->out_limit) output = -Struct_Pid->out_limit;
        return  output ;
    }
    //增量式
    else if(Struct_Pid->mode == Incremental)
    {
        pidtype output = Struct_Pid -> Kp * (error - Struct_Pid ->error_last)
                       + Struct_Pid -> Ki *  error
                       + Struct_Pid -> Kd * (error - 2 * Struct_Pid ->error_last + Struct_Pid ->error_last_last);
        Struct_Pid ->error_last_last = Struct_Pid ->error_last;
        Struct_Pid ->error_last = error;

        Struct_Pid ->output_accumulation += output ;
        //PID输出限幅
        if(Struct_Pid ->output_accumulation > Struct_Pid->out_limit) Struct_Pid ->output_accumulation = Struct_Pid->out_limit;
        if(Struct_Pid ->output_accumulation < -Struct_Pid->out_limit) Struct_Pid ->output_accumulation = -Struct_Pid->out_limit;
        return Struct_Pid ->output_accumulation;
    //防止报错（不信可以删了下面的）
    } else {
        return 0;
    }
}

//------------------------------------------------------------------------------------------------------
// 函数简介     pid计算并输出（error）
// 参数介绍     Struct_Pid      pid目标结构体地址
// 参数介绍     target          目标值
// 参数介绍     current         当前值
// 参数介绍     out_limit       pid输出限幅
// 使用示例
// 返回参数     output                          位置式pid计算结果
// 返回参数     hpid ->output_accumulation      增量式pid计算结果
// 备注信息     数据类型由pid.h中的pidtype定义
//------------------------------------------------------------------------------------------------------
pidtype pid_calculate_error(pidTypedef* Struct_Pid, pidtype error)   //pidtype在文件头定义，可自定义，需要啥就用啥，默认为float
{
    //位置式
    if (Struct_Pid->mode == Positional)
    {
        if (Struct_Pid -> Ki != 0)
            Struct_Pid ->error_integral += error ;                                                    //离散积分累加项
        else
            Struct_Pid ->error_integral = 0;
        pidtype output = Struct_Pid -> Kp * error
                       + clip_pidtype(Struct_Pid -> Ki * Struct_Pid -> error_integral, -Struct_Pid -> integral_limit, Struct_Pid -> integral_limit) //积分限幅(非归一化）：注意积分限幅需要使用原始数据，需要和output同单位
                       + Struct_Pid -> Kd * (error - Struct_Pid ->error_last);
        Struct_Pid->error_last = error;                                                                                          //离散微分需要使用的后向差分项
        //PID输出限幅
        if(output > Struct_Pid ->out_limit) output = Struct_Pid ->out_limit;
        if(output < -Struct_Pid ->out_limit) output = -Struct_Pid ->out_limit;
        return  output ;
    }
    //增量式
    else if(Struct_Pid->mode == Incremental)
    {
        pidtype output = Struct_Pid ->Kp * (error - Struct_Pid ->error_last)
                                 + Struct_Pid ->Ki * error
                                 + Struct_Pid ->Kd * (error - 2 * Struct_Pid ->error_last + Struct_Pid ->error_last_last);
        Struct_Pid ->error_last_last = Struct_Pid ->error_last;
        Struct_Pid ->error_last = error;

        Struct_Pid ->output_accumulation += output ;
        //PID输出限幅
        if(Struct_Pid ->output_accumulation > Struct_Pid ->out_limit) Struct_Pid ->output_accumulation = Struct_Pid ->out_limit;
        if(Struct_Pid ->output_accumulation < -Struct_Pid ->out_limit) Struct_Pid ->output_accumulation = -Struct_Pid ->out_limit;
        return Struct_Pid ->output_accumulation;
    //防止报错（不信可以删了下面的）
    } else {
        return 0;
    }
}

//------------------------------------------------------------------------------------------------------
// 函数简介     pid快速调参（未使用）（其实菜单可以用，但忘了）
// 参数介绍     Struct_Pid      pid目标结构体地址
// 参数介绍     Kp              目标值
// 参数介绍     Ki              当前值
// 参数介绍     Kd              pid输出限幅
// 使用示例     Pid_Fast_Assignment(Struct_Pid_Motor_L, 2, 3, 4);
// 返回参数     void
// 备注信息     数据类型由pid.h中的pidtype定义
//------------------------------------------------------------------------------------------------------
void pid_fast_assignment(pidTypedef* Struct_Pid, pidtype Kp, pidtype Ki, pidtype Kd){
    Struct_Pid->Kp = Kp;
    Struct_Pid->Ki = Ki;
    Struct_Pid->Kd = Kd;
}

//------------------------------------------------------------------------------------------------------
// 函数简介     pid清除增量式累计输出
// 参数介绍     Struct_Pid      pid目标结构体地址
// 使用示例
// 返回参数     void
// 备注信息     数据类型由pid.h中的pidtype定义
//------------------------------------------------------------------------------------------------------
void pid_refresh_output(pidTypedef* Struct_Pid){
    Struct_Pid ->output_accumulation = 0;
    Struct_Pid ->error_last_last = 0;
    Struct_Pid ->error_last = 0;
    Struct_Pid ->error_integral = 0;    //当非连续性调用PID算法时，需要在两次之间刷新所有累计项
}
